#include "../../../XMLTools.h"
#include "../../../MathTools.h"
#include "../../../Tools.h"

#include "MoCapMarkerSensorMeasurement.h"

using namespace MMM;

MoCapMarkerSensorMeasurement::MoCapMarkerSensorMeasurement(float timestep, const std::map<std::string, Eigen::Vector3f> &marker, const std::vector<Eigen::Vector3f> &unlabeledMarker, SensorMeasurementType type) :
    InterpolatableSensorMeasurement(timestep, type),
    markerData(MarkerDataPtr(new MarkerData(marker, unlabeledMarker)))
{
}

MoCapMarkerSensorMeasurement::MoCapMarkerSensorMeasurement(float timestep, MarkerDataPtr markerData, SensorMeasurementType type) :
    InterpolatableSensorMeasurement(timestep, type),
    markerData(markerData)
{
}

SensorMeasurementPtr MoCapMarkerSensorMeasurement::clone() {
    return clone(timestep);
}

bool MoCapMarkerSensorMeasurement::equals(SensorMeasurementPtr sensorMeasurement) {
    MoCapMarkerSensorMeasurementPtr ptr = boost::dynamic_pointer_cast<MoCapMarkerSensorMeasurement>(sensorMeasurement);
    if (ptr) {
        if (!Tools::compare<std::map<std::string, Eigen::Vector3f> >(markerData->getLabeledData(), ptr->markerData->getLabeledData())) return false;
        return markerData->getUnlabeledData() == markerData->getUnlabeledData();
    }
    return false;
}

MoCapMarkerSensorMeasurementPtr MoCapMarkerSensorMeasurement::clone(float newTimestep) {
    MoCapMarkerSensorMeasurementPtr clonedSensorMeasurement(new MoCapMarkerSensorMeasurement(newTimestep, markerData));
    return clonedSensorMeasurement;
}

void MoCapMarkerSensorMeasurement::appendMeasurementDataXML(RapidXMLWriterNodePtr measurementNode) {
    for (const auto &m : markerData->getLabeledData()) {
        RapidXMLWriterNodePtr markerNode = measurementNode->append_node("MarkerPosition");
        markerNode->append_attribute("name", m.first.c_str());
        markerNode->append_data_node(XML::toString(m.second));
    }
    for (auto m : markerData->getUnlabeledData()) {
        measurementNode->append_node("MarkerPosition")->append_data_node((XML::toString(m)));
    }
}

bool MoCapMarkerSensorMeasurement::hasMarker(const std::string &label) {
    return markerData->hasLabel(label);
}

Eigen::Vector3f MoCapMarkerSensorMeasurement::getLabeledMarker(const std::string &label) {
    return markerData->getData(label);
}

std::map<std::string, Eigen::Vector3f> MoCapMarkerSensorMeasurement::getLabeledMarker() {
    return markerData->getLabeledData();
}

std::vector<Eigen::Vector3f> MoCapMarkerSensorMeasurement::getUnlabeledMarker() {
    return markerData->getUnlabeledData();
}

MoCapMarkerSensorMeasurementPtr MoCapMarkerSensorMeasurement::interpolate(MoCapMarkerSensorMeasurementPtr other, float timestep, SensorMeasurementType type) {
    std::map<std::string, Eigen::Vector3f> interpolatedLabeledMarker;
    for (const auto &labeledMarker : other->getLabeledMarker()) {
        std::string markerName = labeledMarker.first;
        if (this->hasMarker(markerName)) {
            interpolatedLabeledMarker[markerName] = Math::linearInterpolation(this->getLabeledMarker(markerName), this->timestep, labeledMarker.second, other->timestep, timestep);
        }
    }
    std::vector<Eigen::Vector3f> interpolatedUnlabeledMarker; // cant be interpolated!
    return MoCapMarkerSensorMeasurementPtr(new MoCapMarkerSensorMeasurement(timestep, interpolatedLabeledMarker, interpolatedUnlabeledMarker, type));
}
